#include "mindvision_camera/mv_camera_sdk.hpp"
#include <libusb-1.0/libusb.h>  // USB重置依赖
#include <stdexcept>  // 异常处理（解析VID/PID时用）
#include <rclcpp/logging.hpp>  // ROS日志

namespace mindvision_camera {

// -------------------------- 构造/析构实现 --------------------------
MVCameraSDK::MVCameraSDK(rclcpp::Logger logger) 
  : logger_(logger), h_camera_(-1), pby_buffer_(nullptr), vid_(-1), pid_(-1){}
MVCameraSDK::~MVCameraSDK() {
  unInit();  // 析构时自动释放资源
}

// -------------------------- 相机生命周期管理实现 --------------------------
bool MVCameraSDK::init() {
  // 1. 先检查是否已初始化
  if (isCameraInitialized()) {
    RCLCPP_WARN(logger_, "Camera already initialized!");
    return true;
  }

  // 2. 初始化SDK（参数1表示初始化所有相机类型）
  CameraSdkInit(1);

  // 3. 枚举相机设备
  int camera_count = 1;
  tSdkCameraDevInfo camera_dev_list;
  int status = CameraEnumerateDevice(&camera_dev_list, &camera_count);
  
  RCLCPP_INFO(logger_, "Camera enumerate result: status=%d, found_count=%d", status, camera_count);
  if (camera_count == 0) {
    RCLCPP_ERROR(logger_, "No MindVision camera found!");
    return false;
  }

  // 4. 初始化相机硬件
  status = CameraInit(&camera_dev_list, -1, -1, &h_camera_);
  RCLCPP_INFO(logger_, "Camera hardware init result: status=%d", status);
  if (status != CAMERA_STATUS_SUCCESS) {
    RCLCPP_ERROR(logger_, "Camera hardware init failed! Status code: %d", status);
    return false;
  }

  // 5. 获取相机能力（参数范围、分辨率等）
  CameraGetCapability(h_camera_, &t_capability_);

  // 6. 配置相机基础参数（ISP输出RGB8、关闭自动曝光）
  CameraSetIspOutFormat(h_camera_, CAMERA_MEDIA_TYPE_RGB8);  // 输出RGB8格式
  CameraSetAeState(h_camera_, false);  // 关闭自动曝光（默认手动模式）

  // 7. 启动相机（进入工作模式）
  CameraPlay(h_camera_);

  RCLCPP_INFO(logger_, "Camera SDK initialized successfully!");
  return true;
}

void MVCameraSDK::unInit() {
  // 1. 停止相机工作
  if (h_camera_ != -1) {
    CameraStop(h_camera_);
  }

  // 2. 释放图像缓冲区（防止内存泄漏）
  releaseImageBuffer();

  // 3. 卸载相机（SDK接口）
  if (h_camera_ != -1) {
    CameraUnInit(h_camera_);
    h_camera_ = -1;
    RCLCPP_INFO(logger_, "Camera SDK uninitialized successfully!");
  }

  // 4. 重置内部变量
  vid_ = -1;
  pid_ = -1;
  t_capability_ = {};
  s_frame_info_ = {};
}

// -------------------------- 参数设置接口实现 --------------------------
bool MVCameraSDK::setExposureTime(double exposure_time) {
  if (!isCameraInitialized()) {
    RCLCPP_ERROR(logger_, "Set exposure failed: camera not initialized!");
    return false;
  }

  // 调用SDK设置曝光时间
  int status = CameraSetExposureTime(h_camera_, exposure_time);
  if (status != CAMERA_STATUS_SUCCESS) {
    RCLCPP_ERROR(logger_, "Set exposure failed! Status: %d, Requested time: %.1f us", status, exposure_time);
    return false;
  }

  return true;
}

bool MVCameraSDK::setAnalogGain(int analog_gain) {
  if (!isCameraInitialized()) {
    RCLCPP_ERROR(logger_, "Set analog gain failed: camera not initialized!");
    return false;
  }

  // 调用SDK设置模拟增益
  int status = CameraSetAnalogGain(h_camera_, analog_gain);
  if (status != CAMERA_STATUS_SUCCESS) {
    RCLCPP_ERROR(logger_, "Set analog gain failed! Status: %d, Requested gain: %d", status, analog_gain);
    return false;
  }

  return true;
}

bool MVCameraSDK::setRgbGain(int r_gain, int g_gain, int b_gain) {
  if (!isCameraInitialized()) {
    RCLCPP_ERROR(logger_, "Set RGB gain failed: camera not initialized!");
    return false;
  }

  // 调用SDK设置RGB增益（三通道同时设置）
  int status = CameraSetGain(h_camera_, r_gain, g_gain, b_gain);
  if (status != CAMERA_STATUS_SUCCESS) {
    RCLCPP_ERROR(logger_, "Set RGB gain failed! Status: %d, R=%d, G=%d, B=%d", status, r_gain, g_gain, b_gain);
    return false;
  }

  return true;
}

bool MVCameraSDK::setSaturation(int saturation) {
  if (!isCameraInitialized()) {
    RCLCPP_ERROR(logger_, "Set saturation failed: camera not initialized!");
    return false;
  }

  // 调用SDK设置饱和度
  int status = CameraSetSaturation(h_camera_, saturation);
  if (status != CAMERA_STATUS_SUCCESS) {
    RCLCPP_ERROR(logger_, "Set saturation failed! Status: %d, Requested value: %d", status, saturation);
    return false;
  }

  return true;
}

bool MVCameraSDK::setGamma(int gamma) {
  if (!isCameraInitialized()) {
    RCLCPP_ERROR(logger_, "Set gamma failed: camera not initialized!");
    return false;
  }

  // 调用SDK设置伽马值
  int status = CameraSetGamma(h_camera_, gamma);
  if (status != CAMERA_STATUS_SUCCESS) {
    RCLCPP_ERROR(logger_, "Set gamma failed! Status: %d, Requested value: %d", status, gamma);
    return false;
  }

  return true;
}

void MVCameraSDK::setVidPid(const std::string &vid_pid) {
  int vid = -1, pid = -1;
  if (parseVidPid(vid_pid, vid, pid)) {
    this->vid_ = vid;
    this->pid_ = pid;
    RCLCPP_INFO(logger_, "VID/PID set to 0x%04X:0x%04X", vid, pid);
  } else {
    RCLCPP_WARN(logger_, "Invalid VID/PID format: %s (expected '0xXXXX:0xXXXX')", vid_pid.c_str());
  }
}

// -------------------------- 图像获取与处理接口实现 --------------------------
bool MVCameraSDK::getImageBuffer(tSdkFrameHead &frame_info, uint8_t *&buffer, int timeout_ms) {
  if (!isCameraInitialized()) {
    RCLCPP_ERROR(logger_, "Get image buffer failed: camera not initialized!");
    return false;
  }

  // 调用SDK获取图像缓冲区（超时返回失败）
  int status = CameraGetImageBuffer(h_camera_, &frame_info, &buffer, timeout_ms);
  if (status != CAMERA_STATUS_SUCCESS) {
    RCLCPP_WARN(logger_, "Get image buffer failed! Status: %d, Timeout: %d ms", status, timeout_ms);
    return false;
  }

  // 缓存当前帧头信息（后续processImage()用）
  this->s_frame_info_ = frame_info;
  this->pby_buffer_ = buffer;

  return true;
}

void MVCameraSDK::processImage(uint8_t *dest_buffer, bool flip_image) {
  if (!isCameraInitialized() || pby_buffer_ == nullptr) {
    RCLCPP_WARN(logger_, "Process image skipped: camera not initialized or buffer null!");
    return;
  }

  // 1. ISP处理：将SDK原始数据转换为RGB8格式（存入外部提供的dest_buffer）
  CameraImageProcess(h_camera_, pby_buffer_, dest_buffer, &s_frame_info_);

  // 2. 图像翻转（如果需要）
  if (flip_image) {
    CameraFlipFrameBuffer(dest_buffer, &s_frame_info_, 3);
    RCLCPP_INFO(logger_, "Image flipped successfully");
  }
}

void MVCameraSDK::releaseImageBuffer() {
  if (h_camera_ != -1 && pby_buffer_ != nullptr) {
    // 调用SDK释放缓冲区（必须与getImageBuffer()配对）
    CameraReleaseImageBuffer(h_camera_, pby_buffer_);
    pby_buffer_ = nullptr; 
  }
}

// -------------------------- 辅助功能接口实现 --------------------------
void MVCameraSDK::resetUsb() {
  if (vid_ == -1 || pid_ == -1) {
    RCLCPP_WARN(logger_, "USB reset failed: VID/PID not set!");
    return;
  }

  // 初始化libusb
  libusb_context *usb_ctx = nullptr;
  int status = libusb_init(&usb_ctx);
  if (status != LIBUSB_SUCCESS) {
    RCLCPP_ERROR(logger_, "USB reset failed: libusb init error! Code: %d", status);
    return;
  }

  // 按VID/PID打开USB设备
  libusb_device_handle *usb_handle = libusb_open_device_with_vid_pid(usb_ctx, vid_, pid_);
  if (!usb_handle) {
    RCLCPP_WARN(logger_, "USB reset failed: cannot open device (VID:0x%04X, PID:0x%04X)", vid_, pid_);
    libusb_exit(usb_ctx);
    return;
  }

  // 重置USB设备
  status = libusb_reset_device(usb_handle);
  if (status == LIBUSB_SUCCESS) {
    RCLCPP_INFO(logger_, "USB device (VID:0x%04X, PID:0x%04X) reset successfully!", vid_, pid_);
  } else {
    RCLCPP_WARN(logger_, "USB reset failed! Code: %d", status);
  }

  // 释放USB资源
  libusb_close(usb_handle);
  libusb_exit(usb_ctx);
}

tSdkCameraCapbility MVCameraSDK::getCapability() const {
  return t_capability_;  // 返回缓存的相机能力结构体
}

int MVCameraSDK::getCurrentAnalogGain() {
  if (!isCameraInitialized()) {
    RCLCPP_WARN(logger_, "Get current analog gain failed: camera not initialized!");
    return 0;
  }

  int gain = 0;
  CameraGetAnalogGain(h_camera_, &gain);  // 调用SDK获取当前增益
  return gain;
}

void MVCameraSDK::getCurrentRgbGain(int &r, int &g, int &b) {
  r = 0;
  g = 0;
  b = 0;

  if (!isCameraInitialized()) {
    RCLCPP_WARN(logger_, "Get current RGB gain failed: camera not initialized!");
    return;
  }

  CameraGetGain(h_camera_, &r, &g, &b);  // 调用SDK获取当前RGB增益
}

int MVCameraSDK::getCurrentSaturation() {
  if (!isCameraInitialized()) {
    RCLCPP_WARN(logger_, "Get current saturation failed: camera not initialized!");
    return 0;
  }

  int saturation = 0;
  CameraGetSaturation(h_camera_, &saturation);  // 调用SDK获取当前饱和度
  return saturation;
}

int MVCameraSDK::getCurrentGamma() {
  if (!isCameraInitialized()) {
    RCLCPP_WARN(logger_, "Get current gamma failed: camera not initialized!");
    return 0;
  }

  int gamma = 0;
  CameraGetGamma(h_camera_, &gamma);  // 调用SDK获取当前伽马值
  return gamma;
}

double MVCameraSDK::getExposureLineTime() {
  if (!isCameraInitialized()) {
    RCLCPP_WARN(logger_, "Get exposure line time failed: camera not initialized!");
    return 0.0;
  }

  double line_time = 0.0;
  CameraGetExposureLineTime(h_camera_, &line_time);  // 调用SDK获取行时间
  return line_time;
}

bool MVCameraSDK::isCameraInitialized() const {
  return h_camera_ != -1;  // 相机句柄 != -1 表示已初始化
}

// -------------------------- 内部辅助函数实现 --------------------------
bool MVCameraSDK::parseVidPid(const std::string &vid_pid, int &vid, int &pid) {
  // 1. 查找分隔符":"
  size_t colon_pos = vid_pid.find(':');
  if (colon_pos == std::string::npos || colon_pos == 0 || colon_pos == vid_pid.size() - 1) {
    return false;
  }

  // 2. 提取VID和PID字符串
  std::string vid_str = vid_pid.substr(0, colon_pos);
  std::string pid_str = vid_pid.substr(colon_pos + 1);

  // 3. 十六进制字符串转整数（支持"0x"前缀）
  try {
    vid = std::stoi(vid_str, nullptr, 16);  // 16表示十六进制
    pid = std::stoi(pid_str, nullptr, 16);
  } catch (const std::invalid_argument &e) {
    RCLCPP_WARN(logger_, "Parse VID/PID failed: invalid hex string! Error: %s", e.what());
    return false;
  } catch (const std::out_of_range &e) {
    RCLCPP_WARN(logger_, "Parse VID/PID failed: value out of range! Error: %s", e.what());
    return false;
  }

  return true;
}

}  // namespace mindvision_camera